
#ifndef ROBOT_SPLINE_HPP
#define ROBOT_SPLINE_HPP

#include "trajectory.hpp"
#include "splineinterpolator.h"

class Spline {
 public:
  Spline();
  ~Spline();
  
  void pushVia(cvec4d* pos);
  void print();
  
  void planPath(JointSpaceSolver* solver, Trajectory* trajectory, uint32_t timestep, uint32_t fidelity); // timestep in ms
 
 private:
  std::vector<cvec4d*>* vias;
};

inline Spline::Spline() {
  vias = new std::vector<cvec4d*>();
}

inline Spline::~Spline() {
  for(uint32_t i = 0; i < vias->size(); i ++) {
    delete vias->at(i);
  }
  delete vias;
}

inline void Spline::pushVia(cvec4d* pos) {
  vias->push_back(new cvec4d(pos));
}

inline void Spline::planPath(JointSpaceSolver* solver, Trajectory* trajectory, uint32_t timestep, uint32_t fidelity) {
  
  // populate vectors to push to the splineinterpolator:
  uint32_t nsize = vias->size();
  
  assert(nsize > 1);
  
  // Make start points have continuous acceleration by adding nearby setpoint:
  cvec4d* cvia0  = vias->at(0);
  cvec4d* cvia1 = vias->at(1);
  cvec4d* clerp = new cvec4d();
  
  cvia0->interpolate(cvia1, 0.05, clerp);
  vias->insert(++vias->begin(), new cvec4d(clerp));
  nsize = vias->size();
  
  cvia0  = vias->at(nsize-2);
  cvia1 = vias->at(nsize-1);
  
  cvia0->interpolate(cvia1, 0.95, clerp);
  vias->insert(--vias->end(), new cvec4d(clerp));
  nsize = vias->size();
  
  delete clerp;
  
  std::vector<double> xvec(nsize), yvec(nsize), 
                      zvec(nsize), pvec(nsize), tvec(nsize);
  
  uint32_t time = 0;
  uint32_t maxtime;
  cvec4d* cvia;
  for(uint32_t i = 0; i < nsize; i ++) {
    cvia = vias->at(i);
    xvec.at(i) = cvia->x;
    yvec.at(i) = cvia->y;
    zvec.at(i) = cvia->z;
    pvec.at(i) = cvia->phi;
    tvec.at(i) = time;
    maxtime = time;
    time += timestep*(i==0?0.05:(i==nsize-2?0.05:1));
  }
  
  // Solve splines in each dimension:
  tk::splineinterpolator xspl, yspl, zspl, pspl;
  xspl.set_points(tvec, xvec);
  yspl.set_points(tvec, yvec);
  zspl.set_points(tvec, zvec);
  pspl.set_points(tvec, pvec);
  
  // Construct Setpoints:
  Setpoint* sp = new Setpoint(0, true);
  
  // Temporary variables:
  double x,y,z,p;
  double x1,y1,z1,p1;
  double xd,yd,zd,pd;
  double xd0,yd0,zd0,pd0;
  double xdd,ydd,zdd,pdd;
  
  // Find spline position, velocity and acceleration:
  for(time = 0; time <= maxtime; time += fidelity) {
    sp->time = time;
    x = xspl(time);
    y = yspl(time);
    z = zspl(time);
    p = pspl(time);
    
    x1 = xspl(time+1);
    y1 = yspl(time+1);
    z1 = zspl(time+1);
    p1 = pspl(time+1);
    
    // Calculate derivative (velocity):
    xd = (x1-x)*1000;  // m/s
    yd = (y1-y)*1000;  // m/s
    zd = (z1-z)*1000;  // m/s
    pd = (p1-p)*1000;  // rad/s
    
    // Calculate double derivative (acceleration):
    xdd = (xd-xd0)*1000; // m/s/s
    ydd = (yd-yd0)*1000; // m/s/s
    zdd = (zd-zd0)*1000; // m/s/s
    pdd = (pd-pd0)*1000; // rad/s/s
    
    sp->cpos->set(x, y, z, p);
    sp->cvel->set(xd, yd, zd, pd);
    sp->cacc->set(xdd, ydd, zdd, pdd);
    
    solver->solve(sp);
    trajectory->addSetpoint(sp);
    
    // Save velocities for next step:
    xd0 = xd;
    yd0 = yd;
    zd0 = zd;
    pd0 = pd;
  }
  
  delete sp;
}

inline void Spline::print() {
  uint32_t nsize = vias->size();
  for(uint32_t i = 0; i < nsize; i ++) {
    vias->at(i)->println();
  }
}

#endif
